#include "my_lib/menu.h"

//原点
#define OriPoint_X_2  94
#define OriPOint_Y_2  98.5


//原点
#define OriPoint_X  95
#define OriPOint_Y  98.5
//左上
#define TopLeft_X   108
#define TopLeft_Y   90
//右上
#define TopRight_X  83
#define TopRight_Y  90
//右下
#define DownRight_X 82
#define DownRight_Y 107.2
//左下
#define DownLeft_X  108
#define DownLeft_Y  107.2

point_angle_t point_angle_arr[5] = {
    {OriPoint_X, OriPOint_Y},
    {TopLeft_X, TopLeft_Y},
    {TopRight_X, TopRight_Y},
    {DownRight_X, DownRight_Y},
    {DownLeft_X, DownLeft_Y}
};

uint8_t task_id = 0;

void MainMenu(void);
void MeunSub1(void);
void MeunSub2(void);
void MeunSub3(void);
void MeunSub4(void);
void MeunSub5(void);
void MeunSub5_1(void);
void MeunSub5_2(void);
void MeunSub5_3(void);
void Sub1_Fun(void);
void Sub2_Fun(void);
void Sub3_Fun(void);
void Sub4_Fun(void);
void Sub5_1_Fun(void);
void Sub5_2_Fun(void);
void Sub5_3_Fun(void);

uint8_t current_menu_index = 0;  // 当前菜单索引号



//连接各个界面
MenuItem menu_items[MAX_MENU_ITEMS] = {
    //初始界面
    {0, 0,  0, 1, 0, MainMenu},
    //第一层
    {1, 5, 2, 9, 0, MeunSub1},
    {2, 1, 3, 10, 0, MeunSub2},
    {3, 2, 4, 11, 0, MeunSub3},
    {4, 3, 5, 12, 0, MeunSub4},
    {5, 4, 1, 6,  0, MeunSub5},
    // 第二层
    {6, 8, 7, 13, 5, MeunSub5_1},
    {7, 6, 8, 14, 5, MeunSub5_2},
    {8, 7, 6, 15, 5, MeunSub5_3},
    //功能项
    {9, 0, 0, 0, 1, Sub1_Fun},
    {10, 0, 0, 0, 2, Sub2_Fun},
    {11, 0, 0, 0, 3, Sub3_Fun},
    {12, 0, 0, 0, 4, Sub4_Fun},
    {13, 0, 0, 0, 6, Sub5_1_Fun},
    {14, 0, 0, 0, 7, Sub5_2_Fun},
    {15, 0, 0, 0, 8, Sub5_3_Fun}
};

//初始化界面
void MainMenu(void)
{
    OLED_Clear();
    OLED_ShowString(0,0,"welcome!!!", 16, 1);
    OLED_ShowString(0,16*2,"press enter...", 16, 1);
    OLED_Refresh();
}
//子菜单
void MeunSub1(void)
{
    OLED_Clear();
    OLED_ShowString(0,0,"reset", 16, 0);
    OLED_ShowString(0,16,"fun 2", 16, 1);
    OLED_ShowString(0,32,"fun 3", 16, 1);
    OLED_ShowString(0,48,"fun 4", 16, 1);
    OLED_Refresh();
    // HAL_GPIO_WritePin(JIGUANG_GPIO_Port, JIGUANG_Pin, GPIO_PIN_SET);
}
void MeunSub2(void)
{
    OLED_Clear();
    OLED_ShowString(0,0,"reset", 16, 1);
    OLED_ShowString(0,16,"fun 2", 16, 0);
    OLED_ShowString(0,32,"fun 3", 16, 1);
    OLED_ShowString(0,48,"fun 4", 16, 1);
    OLED_Refresh();
}
void MeunSub3(void)
{
    OLED_Clear();
    OLED_ShowString(0,0,"reset", 16, 1);
    OLED_ShowString(0,16,"fun 2", 16, 1);
    OLED_ShowString(0,32,"fun 3", 16, 0);
    OLED_ShowString(0,48,"fun 4", 16, 1);
    OLED_Refresh();
}
void MeunSub4(void)
{
    OLED_Clear();
    OLED_ShowString(0,0,"reset", 16, 1);
    OLED_ShowString(0,16,"fun 2", 16, 1);
    OLED_ShowString(0,32,"fun 3", 16, 1);
    OLED_ShowString(0,48,"fun 4", 16, 0);
    OLED_Refresh();
}
void MeunSub5(void)
{
    OLED_Clear();
    OLED_ShowString(0,0,"fun 2", 16, 1);
    OLED_ShowString(0,16,"fun 3", 16, 1);
    OLED_ShowString(0,32,"fun 4", 16, 1);
    OLED_ShowString(0,48,"config", 16, 0);
    OLED_Refresh();
}
void MeunSub5_1(void)
{
    OLED_Clear();
    OLED_ShowString(0,0,"point adjust", 16, 0);
    OLED_ShowString(0,16,"reset 2", 16, 1);
    OLED_ShowString(0,32,"config 3", 16, 1);
    OLED_Refresh();
}
void MeunSub5_2(void)
{
    OLED_Clear();
    OLED_ShowString(0,0,"point adjust", 16, 1);
    OLED_ShowString(0,16,"reset 2", 16, 0);
    OLED_ShowString(0,32,"config 3", 16, 1);
    OLED_Refresh();
}
void MeunSub5_3(void)
{
    OLED_Clear();
    OLED_ShowString(0,0,"point adjust", 16, 1);
    OLED_ShowString(0,16,"reset 2", 16, 1);
    OLED_ShowString(0,32,"config 3", 16, 0);
    OLED_Refresh();
}
//子菜单功能执行
void Sub1_Fun(void)    //中心点复位
{
    OLED_Clear();
    OLED_ShowString(0,0,"-----reset------", 16, 1);
    OLED_Refresh();
    MY_LOGI("menu", "fun 1");
    SetBeepFun(0, BeepOnMement, 200);
    SteerAngle2PWM(HorizontalSteer, OriPoint_X);
    SteerAngle2PWM(VerticalSteer, OriPOint_Y);
}

void Sub2_Fun(void)  //扫线正方形
{
    OLED_Clear();
    OLED_ShowString(0,0,"-----fun 2------", 16, 1);
    OLED_Refresh();
    MY_LOGI("menu", "fun 2");
    SetBeepFun(0, BeepOnMement, 200);

    //回到原点
    MY_LOGI("fun2", "reset point");
    SetSteerAngle(HorizontalSteer, OriPoint_X);
    SetSteerAngle(VerticalSteer, OriPOint_Y);
    while(steers[HorizontalSteer].state || steers[VerticalSteer].state){
        vTaskDelay(2);
    }
    vTaskDelay(500);
    //一点
    MY_LOGI("fun2", "point 1");
    SetSteerAngle(HorizontalSteer, TopLeft_X);
    SetSteerAngle(VerticalSteer, TopLeft_Y);
    while(steers[HorizontalSteer].state || steers[VerticalSteer].state){
        vTaskDelay(2);
    }
    vTaskDelay(500);
    //二点
    MY_LOGI("fun2", "point 2");
    SetSteerAngle(HorizontalSteer, TopRight_X);
    SetSteerAngle(VerticalSteer, TopRight_Y);
    while(steers[HorizontalSteer].state || steers[VerticalSteer].state){
        vTaskDelay(2);
    }
    vTaskDelay(500);
    //三点
    MY_LOGI("fun2", "point 3");
    SetSteerAngle(HorizontalSteer, DownRight_X);
    SetSteerAngle(VerticalSteer, DownRight_Y);
    while(steers[HorizontalSteer].state || steers[VerticalSteer].state){
        vTaskDelay(2);
    }
    vTaskDelay(500);
    //四点
    MY_LOGI("fun2", "point 4");
    SetSteerAngle(HorizontalSteer, DownLeft_X);
    SetSteerAngle(VerticalSteer, DownLeft_Y);
    while(steers[HorizontalSteer].state || steers[VerticalSteer].state){
        vTaskDelay(2);
    }
    vTaskDelay(500);
    //回到一点
    MY_LOGI("fun2", "point 1");
    SetSteerAngle(HorizontalSteer, TopLeft_X);
    SetSteerAngle(VerticalSteer, TopLeft_Y);
    while(steers[HorizontalSteer].state || steers[VerticalSteer].state){
        vTaskDelay(2);
    }
    MY_LOGI("fun2", "over");
    OLED_Clear();
    OLED_ShowString(0,0,"fun 2 over!!!", 16, 1);
    OLED_Refresh();
}
void Sub3_Fun(void)  //功能3
{
    OLED_Clear();
    OLED_ShowString(0,0,"-----fun 3------", 16, 1);
    OLED_Refresh();
    MY_LOGI("menu", "fun 3");  
    SetBeepFun(0, BeepOnMement, 100);
    task_id = 3;
    char cmd = '3';         //对树莓派发送执行指令
    TransBytes(&cmd, 1);
    uint8_t x_sign, y_sign;
    int16_t x_bias, y_bias;
    IsPointfollow(1);   //开启巡点功能
    // SteerAngle2PWM(HorizontalSteer, 109); //移动位置
    // SteerAngle2PWM(VerticalSteer, 92);
    // vTaskDelay(100);
    while(1)
    {
        if(AskButtonEvent(0, 0) == BTN_Long){
            extern void ShuMeiPiRst(double param);
            ShuMeiPiRst(0);

            SetPointBias(0, 0);
            IsPointfollow(0);   //关闭巡点功能
            break;
        }
        //通信追点
        if(xSemaphoreTake(com_receive_sem, 0) == pdTRUE)
        {
            //point follow pid
            if(com_infor.id == FOLLOW_STOP){  //停止信号
                ResetInfo();
                SetBeepFun(0, BeepOnMement, 1000);
                OLED_Clear();        
                OLED_ShowString(0,0,"fun 3 over!!!", 16, 1);
                OLED_Refresh();
                SetPointBias(0, 0);
                IsPointfollow(0);   //关闭巡点功能
                break;
            }
            else if(com_infor.id != POINT_FOLLOW_ID){
                MY_LOGE("follow", "id[%d] is invalid", com_infor.id);
                continue;
            }
            x_sign = com_infor.infor_buf[0];
            x_bias = com_infor.infor_buf[1]*100;
            x_bias += com_infor.infor_buf[2];
            x_bias = x_sign? (-x_bias):x_bias;
            y_sign = com_infor.infor_buf[3];
            y_bias = com_infor.infor_buf[4]*100;
            y_bias += com_infor.infor_buf[5];
            y_bias = y_sign? (-y_bias):y_bias;

            SetPointBias(x_bias, y_bias);
            //temp test
            MY_LOG_Print("bias:x[%d] y[%d]\r\n", x_bias, y_bias);
        }
        vTaskDelay(10);
    }
}
void Sub4_Fun(void)  //功能4
{
    OLED_Clear();
    OLED_ShowString(0,0,"-----fun 4------", 16, 1);
    OLED_Refresh();
    MY_LOGI("menu", "fun 4");
    SetBeepFun(0, BeepOnMement, 100);
    task_id = 4;
    char cmd = '4';         //对树莓派发送执行指令
    TransBytes(&cmd, 1);
    uint8_t x_sign, y_sign;
    int16_t x_bias, y_bias;
    IsPointfollow(1);   //开启巡点功能
    while(1)
    {
        if(AskButtonEvent(0, 0) == BTN_Long){
            extern void ShuMeiPiRst(double param);
            ShuMeiPiRst(0);

            SetPointBias(0, 0);
            IsPointfollow(0);   //关闭巡点功能
            break;
        }
        //通信追点
        if(xSemaphoreTake(com_receive_sem, 0) == pdTRUE)
        {
            //point follow pid
            if(com_infor.id == FOLLOW_STOP){  //停止信号
                ResetInfo();
                SetBeepFun(0, BeepOnMement, 1000);
                OLED_Clear();        
                OLED_ShowString(0,0,"fun 4 over!!!", 16, 1);
                OLED_Refresh();
                SetPointBias(0, 0);
                IsPointfollow(0);   //关闭巡点功能
                break;
            }
            else if(com_infor.id != POINT_FOLLOW_ID){
                MY_LOGE("follow", "id[%d] is invalid", com_infor.id);
                continue;
            }
            x_sign = com_infor.infor_buf[0];
            x_bias = com_infor.infor_buf[1]*100;
            x_bias += com_infor.infor_buf[2];
            x_bias = x_sign? (-x_bias):x_bias;
            y_sign = com_infor.infor_buf[3];
            y_bias = com_infor.infor_buf[4]*100;
            y_bias += com_infor.infor_buf[5];
            y_bias = y_sign? (-y_bias):y_bias;
            SetPointBias(x_bias, y_bias);
            //temp test
            MY_LOG_Print("bias:x[%d] y[%d]\r\n", x_bias, y_bias);
        }
        vTaskDelay(10);
    }
}
void Sub5_1_Fun(void)
{
    static uint8_t point_index =0;
    OLED_Clear();        
    OLED_ShowString(0,0,"--point adjust--", 16, 1);
    OLED_ShowString(0,16*2,"point->", 16, 1);
    OLED_ShowNum(8*8,16*2, point_index, 1, 16, 1);
    OLED_Refresh();
    MY_LOGI("menu", "config 1");
    while(1)
    {
        if(AskButtonEvent(0,0) == BTN_Short){
            AskButtonEvent(0, 1);
            if(point_index > 0){
                point_index--;
            }
            else{
                point_index = 4;
            }
            OLED_ShowNum(8*8,16*2, point_index, 1, 16, 1);
            OLED_Refresh();
            SetSteerAngle(HorizontalSteer, point_angle_arr[point_index].HorizontalAngle);   
            SetSteerAngle(VerticalSteer, point_angle_arr[point_index].VerticalAngle);
        }
        else if(AskButtonEvent(0, 1) == BTN_Long){
            break;
        }

        if(AskButtonEvent(1,1) == BTN_Short){
            if(point_index < 4){
                point_index++;
            }
            else{
                point_index = 0;
            }
            OLED_ShowNum(8*8,16*2, point_index, 1, 16, 1);
            OLED_Refresh();
            SetSteerAngle(HorizontalSteer, point_angle_arr[point_index].HorizontalAngle);   
            SetSteerAngle(VerticalSteer, point_angle_arr[point_index].VerticalAngle);
        }
        vTaskDelay(10);
    }
}
void Sub5_2_Fun(void)
{
    OLED_Clear();
    OLED_ShowString(0,0,"-----reset 2----", 16, 1);
    OLED_Refresh();
    MY_LOGI("menu", "reset 2");
    SetBeepFun(0, BeepOnMement, 200);
    SteerAngle2PWM(HorizontalSteer, OriPoint_X_2);
    SteerAngle2PWM(VerticalSteer, OriPOint_Y_2);
}
void Sub5_3_Fun(void)
{
    OLED_Clear();       
    OLED_ShowString(0,0,"----config 3----", 16, 1);
    OLED_Refresh();
    MY_LOGI("menu", "config 3");
    while(1)
    {
        if(AskButtonEvent(0, 0) == BTN_Long){
            break;
        }
        vTaskDelay(10);
    }
}


void MenuTask(void* param)
{
    uint16_t count = 0;
    OLED_Init();
    OLED_ShowString(0,0,"waiting......", 16, 1);
    OLED_Refresh();
    MY_LOGI("SM Pi", " waiting start");
    char cmd = '6';         //对树莓派发送执行指令
    TransBytes(&cmd, 1);
    while(xSemaphoreTake(com_receive_sem, 0) != pdTRUE)  //死等树莓派启动
    {
        MY_LOGI("SM Pi", " waiting start[%d]", count++);
        vTaskDelay(200);
        TransBytes(&cmd, 1);
    }
    SetBeepFun(0, BeepOnMement, 100);
    if(com_infor.id != FOLLOW_READY){
        MY_LOGE("SM Pi", "start error");
    }
    else{
        MY_LOGI("SM Pi", "start");
    }

    menu_items[0].function();
    while (1)
    {
        if(AskButtonEvent(0, 0) == BTN_Short){
            AskButtonEvent(0, 1);  //复位
            current_menu_index = menu_items[current_menu_index].up_id;
            menu_items[current_menu_index].function();
            MY_LOGI("button", "b0 short");
        }
        else if(AskButtonEvent(0, 1) == BTN_Long){
            current_menu_index = menu_items[current_menu_index].return_id;
            menu_items[current_menu_index].function();
            MY_LOGI("button", "b0 long");
        }

        if(AskButtonEvent(1, 0) == BTN_Short){
            AskButtonEvent(1, 1);  //复位
            current_menu_index = menu_items[current_menu_index].down_id;
            menu_items[current_menu_index].function();
            MY_LOGI("button", "b1 short");
        }
        else if(AskButtonEvent(1, 1) == BTN_Long){
            current_menu_index = menu_items[current_menu_index].enter_id;
            menu_items[current_menu_index].function();
            MY_LOGI("button", "b1 long");
        }

        // if(AskButtonEvent(2, 0) == BTN_Short){
        //     AskButtonEvent(2, 1);  //复位
        //     current_menu_index = menu_items[current_menu_index].enter_id;
        //     menu_items[current_menu_index].function();
        //     MY_LOGI("button", "b2 short");
        // }
        // else if(AskButtonEvent(2, 1) == BTN_Long){
        //     current_menu_index = menu_items[current_menu_index].return_id;
        //     menu_items[current_menu_index].function();
        //     MY_LOGI("button", "b2 long");
        // }
        vTaskDelay(20);
    }
}




